#ifndef __PARTICLE_CONST_H__
#define __PARTICLE_CONST_H__

#include "Eigen/Dense"
#include <vector>

using namespace std;

//#define ALGORITHMS_SIMULATE

#ifdef ALGORITHMS_SIMULATE
	#define OPENCL_KERNEL_SIMULATE_PSO		"matchingSimulatedModel"
#endif // ALGORITHMS_SIMULATE
	
	#define OPENCL_FILE				"PSO_kernel.cl"
	#define OPENCL_KERNEL_PSO		"matchingObserve"
	#define OPENCL_KERNEL_REDUCTION	"reduction_vector"

	#define DMAX	(40)	//dM - 40mm
	#define Dmax	(10)	//dm - 10mm
	#define LAMDA	(20)
	#define LAMDA_K	(10)

//Chia 64 particles thanh 8 groups de ve cho vua man hinh
//Voi size to 128*128*64 can su dung ki thuat off-screen render
#define SIZE_IMAGE_PARTICLE_WIDTH		128
#define SIZE_IMAGE_PARTICLE_HEIGHT		128
#define NUM_TOTAL_PARTICLES				64

#define K_WINDOWS_WIDTH					640
#define K_WINDOWS_HEIGHT				480

//DOFs POSITION KEYS IN RENDERING
#define OFFSET_WRIST	0	// +3 Position +3 Rotation
#define OFFSET_THUMB	6	// +4 thumb_ROTATION
#define OFFSET_FINGER1	10
#define OFFSET_FINGER2	14
#define OFFSET_FINGER3	18
#define OFFSET_LITTLE	22
#define K_NUM_DOF		26

//DOFs POSITION KEYS IN PSO VECTOR
#define vPOS_WRIST		0
#define vPOS_THUMB		6
#define vPOS_FINGER1	10
#define vPOS_FINGER2	13
#define vPOS_FINGER3	16
#define vPOS_LITTLE		19
#define vPOS_DOF		22

struct Hypothesis
{
	float dofData[26];
};

inline void ShowHypo(Hypothesis hypo)
{
	//printf("wirst_Position: %.2f %.2f %.2f\n", hypo.wirst_Position[0],hypo.wirst_Position[1], hypo.wirst_Position[2]);
	printf("wirst_Rotation: %.2f %.2f %.2f\n", hypo.dofData[OFFSET_WRIST+0],hypo.dofData[OFFSET_WRIST+1], hypo.dofData[OFFSET_WRIST+2]);
	printf("thumb_Rotation: %.2f %.2f %.2f %.2f\n", hypo.dofData[OFFSET_THUMB+0],hypo.dofData[OFFSET_THUMB+1], hypo.dofData[OFFSET_THUMB+2], hypo.dofData[OFFSET_THUMB+3]);
	printf("finger1_Rotation: %.2f %.2f %.2f %.2f\n", hypo.dofData[OFFSET_FINGER1+0],hypo.dofData[OFFSET_FINGER1+1], hypo.dofData[OFFSET_FINGER1+2], hypo.dofData[OFFSET_FINGER1+3]);
	printf("finger2_Rotation: %.2f %.2f %.2f %.2f\n", hypo.dofData[OFFSET_FINGER2+0],hypo.dofData[OFFSET_FINGER2+1], hypo.dofData[OFFSET_FINGER2+2], hypo.dofData[OFFSET_FINGER2+3]);
	printf("finger3_Rotation: %.2f %.2f %.2f %.2f\n", hypo.dofData[OFFSET_FINGER3+0],hypo.dofData[OFFSET_FINGER3+1], hypo.dofData[OFFSET_FINGER3+2], hypo.dofData[OFFSET_FINGER3+3]);
	printf("little_Rotation: %.2f %.2f %.2f %.2f\n", hypo.dofData[OFFSET_LITTLE+0],hypo.dofData[OFFSET_LITTLE+1], hypo.dofData[OFFSET_LITTLE+2], hypo.dofData[OFFSET_LITTLE+3]);
	return;
};


inline vector<double> HypoToPosition(Hypothesis hypo)
{
	vector<double> position(vPOS_DOF);

	for (int i=0;i<3;i++) {
		position[vPOS_WRIST+i] = hypo.dofData[OFFSET_WRIST+i];
		position[vPOS_WRIST+i+3] = hypo.dofData[OFFSET_WRIST+i+3];
		position[vPOS_THUMB+i] = hypo.dofData[OFFSET_THUMB+i];
		position[vPOS_FINGER1+i] = hypo.dofData[OFFSET_FINGER1+i];
		position[vPOS_FINGER2+i] = hypo.dofData[OFFSET_FINGER2+i];
		position[vPOS_FINGER3+i] = hypo.dofData[OFFSET_FINGER3+i];
		position[vPOS_LITTLE+i] = hypo.dofData[OFFSET_LITTLE+i];
	}
	position[vPOS_THUMB+3] = hypo.dofData[OFFSET_THUMB+3];
	return position;
};

inline Hypothesis PositionToHypo(vector<double> position)
{
	Hypothesis hypo;
	for (int i=0;i<3;i++) {
		hypo.dofData[OFFSET_WRIST+i] = (float)position[vPOS_WRIST+i];
		hypo.dofData[OFFSET_WRIST+i+3] = (float)position[vPOS_WRIST+i+3];
		hypo.dofData[OFFSET_THUMB+i] = (float)position[vPOS_THUMB+i];
		hypo.dofData[OFFSET_FINGER1+i] = (float)position[vPOS_FINGER1+i];
		hypo.dofData[OFFSET_FINGER2+i] = (float)position[vPOS_FINGER2+i];
		hypo.dofData[OFFSET_FINGER3+i] = (float)position[vPOS_FINGER3+i];
		hypo.dofData[OFFSET_LITTLE+i] = (float)position[vPOS_LITTLE+i];
	}
	hypo.dofData[OFFSET_THUMB+3] = (float)position[vPOS_THUMB+3];
	hypo.dofData[OFFSET_FINGER1+3] = (0.67f)*(float)position[vPOS_FINGER1+2];
	hypo.dofData[OFFSET_FINGER2+3] = (0.67f)*(float)position[vPOS_FINGER2+2];
	hypo.dofData[OFFSET_FINGER3+3] = (0.67f)*(float)position[vPOS_FINGER3+2];
	hypo.dofData[OFFSET_LITTLE+3] = (0.67f)*(float)position[vPOS_LITTLE+2];

	return	hypo;
};

#endif